
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_control.h
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "gp_mathlib.h"

#include <common/gp_defines.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// update_vel_accel - single axis projection of velocity, vel, forwards in time based on a time step of dt and acceleration of accel.
// the velocity is not moved in the direction of limit if limit is not set to zero
void control_update_vel_accel(float* vel, float accel, float dt, float limit);

// update_pos_vel_accel - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// the position and velocity is not moved in the direction of limit if limit is not set to zero
void control_update_pos_vel_accel(postype_t* pos, float* vel, float accel, float dt, float limit);

// update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// the velocity is not moved in the direction of limit if limit is not set to zero
void control_update_vel_accel_xy(Vector2f_t* vel, const Vector2f_t* accel, float dt, Vector2f_t limit);

// update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// the position and velocity is not moved in the direction of limit if limit is not set to zero
void control_update_pos_vel_accel_xy(Vector2p* pos, Vector2f_t* vel, const Vector2f_t* accel, float dt, Vector2f_t limit);

/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
 The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
 The kinematic path is constrained by :
     acceleration limits - accel_min, accel_max,
     time constant - tc.
 The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
 The time constant also defines the time taken to achieve the maximum acceleration.
 The time constant must be positive.
 The function alters the variable accel to follow a jerk limited kinematic path to accel_input
*/
void control_shape_accel(float accel_input, float* accel, float jerk_max, float dt);

// 2D version
void control_shape_accel_xy(const Vector2f_t* accel_input, Vector2f_t* accel, float jerk_max, float dt);

void control_shape_accel_xy2(const Vector3f_t* accel_input, Vector3f_t* accel, float jerk_max, float dt);

/* shape_vel_accel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
 The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
 The kinematic path is constrained by :
     maximum velocity - vel_max,
     maximum acceleration - accel_max,
     time constant - tc.
 The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
 The time constant also defines the time taken to achieve the maximum acceleration.
 The time constant must be positive.
 The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input
 The accel_max limit can be removed by setting it to zero.
*/
void control_shape_vel_accel(float vel_input, float accel_input,
                     float vel, float* accel,
                     float accel_min, float accel_max,
                     float jerk_max, float dt, bool limit_total_accel);

// 2D version
void control_shape_vel_accel_xy(const Vector2f_t *vel_input1, const Vector2f_t* accel_input,
                     const Vector2f_t* vel, Vector2f_t* accel,
                     float accel_max, float jerk_max, float dt, bool limit_total_accel);

/* shape_pos_vel_accel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by :
  maximum velocity - vel_max,
  maximum acceleration - accel_max,
  time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
*/
void control_shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input,
                      postype_t pos, float vel, float* accel,
                      float vel_min, float vel_max,
                      float accel_min, float accel_max,
                      float jerk_max, float dt, bool limit_total_accel);

// 2D version
void control_shape_pos_vel_accel_xy(const Vector2p* pos_input, const Vector2f_t* vel_input, const Vector2f_t* accel_input,
                      const Vector2p* pos, const Vector2f_t* vel, Vector2f_t* accel,
                      float vel_max, float accel_max,
                      float jerk_max, float dt, bool limit_total_accel);

// calculate the stopping distance for the square root controller based deceleration path
float control_stopping_distance(float velocity, float p, float accel_max);

// calculate the maximum acceleration or velocity in a given direction
// based on horizontal and vertical limits.
float control_kinematic_limit(Vector3f_t direction, float max_xy, float max_z_pos, float max_z_neg);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



